// Algorithm from http://www.movable-type.co.uk/scripts/latlong.html
package com.triangulator.gps;

public class Intersection {	
	public static double toRad(double degree) 	{ return degree * Math.PI/180; }
	public static double toDeg(double rad) 		{ return rad * 180/Math.PI; }

	public static LatLong intersect(LatLong loc1, LatLong loc2) {
		double orient1 = loc1.getOrientation();
		double orient2 = loc2.getOrientation();

		double lat1 = Intersection.toRad(loc1.getLatitude());
		double lon1 = Intersection.toRad(loc1.getLongitude());
		double lat2 = Intersection.toRad(loc2.getLatitude());
		double lon2 = Intersection.toRad(loc2.getLongitude());

		double dLat = lat2 - lat1;
		double dLon = lon2 - lon1;

		double dist12 = 2*Math.asin( Math.sqrt( Math.sin(dLat/2)*Math.sin(dLat/2) + 
				Math.cos(lat1)*Math.cos(lat2)*Math.sin(dLon/2)*Math.sin(dLon/2) ) );

		if (dist12 == 0)
			return null;

		// initial/final bearings between points
		double brngA = Math.acos( ( Math.sin(lat2) - Math.sin(lat1)*Math.cos(dist12) ) / 
				( Math.sin(dist12)*Math.cos(lat1) ) );


		double brngB = Math.acos( ( Math.sin(lat1) - Math.sin(lat2)*Math.cos(dist12) ) / 
				( Math.sin(dist12)*Math.cos(lat2) ) );

		double brng12, brng21;

		if (Math.sin(lon2-lon1) > 0) {
			brng12 = brngA;
			brng21 = 2*Math.PI - brngB;
		} else {
			brng12 = 2*Math.PI - brngA;
			brng21 = brngB;
		}

		double alpha1 = (orient1 - brng12 + Math.PI) % (2*Math.PI) - Math.PI;  // angle 2-1-3
		double alpha2 = (brng21 - orient2 + Math.PI) % (2*Math.PI) - Math.PI;  // angle 1-2-3

		if (Math.sin(alpha1)==0 && Math.sin(alpha2)==0) return null;  // infinite intersections
		if (Math.sin(alpha1)*Math.sin(alpha2) < 0) return null;       // ambiguous intersection

		double alpha3 = Math.acos( -Math.cos(alpha1)*Math.cos(alpha2) + 
				Math.sin(alpha1)*Math.sin(alpha2)*Math.cos(dist12) );
		double dist13 = Math.atan2( Math.sin(dist12)*Math.sin(alpha1)*Math.sin(alpha2), 
				Math.cos(alpha2)+Math.cos(alpha1)*Math.cos(alpha3) );
		double lat3 = Math.asin( Math.sin(lat1)*Math.cos(dist13) + 
				Math.cos(lat1)*Math.sin(dist13)*Math.cos(orient1) );
		double dLon13 = Math.atan2( Math.sin(orient1)*Math.sin(dist13)*Math.cos(lat1), 
				Math.cos(dist13)-Math.sin(lat1)*Math.sin(lat3) );
		double lon3 = lon1+dLon13;
		
		lon3 = (lon3+Math.PI) % (2*Math.PI) - Math.PI;  // normalise to -180..180�

		return new LatLong(Intersection.toDeg(lat3), Intersection.toDeg(lon3));
	}
}